#include <opencv2/opencv.hpp>
#include <yaml-cpp/yaml.h>

#include "device/device.h"
#include "processing/processing.h"
#include "tracking/tracking.h"
#include "control/Controller.h"
#include "udp/udp.h"
#include "utils/pprint.hpp"

using namespace std;

int main(int argc, char* argv[]) {
    string configPath("./data/config.yaml");
    auto config = YAML::LoadFile(configPath);

    auto mode = config["mode"].as<string>();
    if (mode == "camera") {
        Camera camera;
        camera.open(0, 800, 600, 25);
    }
    else if (mode == "video") {
        Video video;
        auto videoPath = config["video path"].as<string>();
        video.open(videoPath.c_str());
    }
    else {
        cerr << "Mode ERROR!" << endl;
        return -1;
    }

    //Recorder recorder;
    //recorder.open();

    auto anchorPath = config["saved anchors path"].as<string>();
    calibrate(anchorPath.c_str());

    auto labelMatrixPath = config["label matrix path"].as<string>();
    loadLabelMatrix(labelMatrixPath.c_str());

    cv::Mat frame;
    readFrame(frame);
    perspectiveTransform(frame);

    MultipleObjectTracker mot;
    mot.init(frame);

    Controller controller;
    vector<vector<int>> routes = config["routes"].as<vector<vector<int>>>();
    controller.init(routes);
    auto ip = config["remote ip"].as<string>();
    auto port = config["remote port"].as<int>();
    controller.udp.open(ip.c_str(), port);

    for (int i = 0; i < 1000; ++i) {
        readFrame(frame);
        perspectiveTransform(frame);
        mot.update(frame);
        controller.update();
        controller.sendCommand();
    }

    return 0;
}